/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: JointGroupCtrlClient.cpp,v 1.18 2009-10-23 13:56:43 morsko Exp $
 *
 * \file
 * \brief Implementation for JointGroupCtrl interface client library.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 *
 */

#include "JointGroupCtrlClient.hpp"


//*****************************************************************************
using namespace MaCI::JointGroupCtrl;
//*****************************************************************************
//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
CJointGroupCtrlClient::CJointGroupCtrlClient(gimi::GIMI *aGIMIPtr, 
					     const int aInterfaceMinor): 
  CSync(8,1),
  CThread(1),
  CMaCI(aGIMIPtr, GIMI_PROTOCOL_MACI_JOINTGROUPCTRL, aInterfaceMinor),
  iReplyMessages(),
  iCmdSequence(0),
  iSpeedData(),
  iSpeedDataSequence(0),
  iPositionData(),
  iPositionDataSequence(0),
  iForceData(),
  iForceDataSequence(0),
  iEventTypes(0)
{
  dPrint(4,"CJointGroupCtrlClient %p constructed", this);
}
//*****************************************************************************

CJointGroupCtrlClient::~CJointGroupCtrlClient()
{
  DoClose();
  dPrint(4,"CSpeedCtrlClient %p destructed", this);
}
//*****************************************************************************

MaCI::EMaCIError CJointGroupCtrlClient::DoOpen(void)
{

  EMaCIError result = KMaCIError;
  if(!IsActive()){
    // Do Subs. using the MaCI provided function.
    result = SubscribeToInterface();
   
    if(result == KMaCIOK){
      ///Get event type
      if(!GetEventTypes()){
        dPrint(3,"Failed to get event types from server");
        return KMaCIError ;
      }else{
        dPrint(1,"GOT EVENT TYPES... %d",iEventTypes.continuousTypes);
        if(iEventTypes.continuousTypes != 0){
          RequestStateOfServer();
        }
      }

      //All ok => Start thread
      SetActive(true);
      CThread::RunThread();
      result = KMaCIOK;
      dPrint(4,"CJointGroupCtrlClient::DoOpen() was succesfull"); 
	
    }
	
  }else{
    dPrint(3,"Client already opened. Not opening it again.");
  }
  return result;

}
//*****************************************************************************

MaCI::EMaCIError CJointGroupCtrlClient::DoClose(void)
{
  EMaCIError result = KMaCIError;
  if(IsActive()){
    SetActive(false);
    /* CThread::CancelThread(); //Removed by script */
   
    result = UnsubscribeToInterface(TimeoutValue(2000));
  
    //Notify all!
    Notify(KReplyJointInfo);
    Notify(KReplySpeed);
    Notify(KReplyPosition);
    Notify(KReplyJointGroupStatus);
    Notify(KReplyForce);
    Notify(KGroupSpeeds);
    Notify(KGroupPositions);
    Notify(KGroupForces);

    //Wait for thread
    CThread::WaitThread();
    if(result == KMaCIOK){
      dPrint(4,"Succesfully closed JointGroupCtrlClient");    
    }else{
      dPrint(4,"Unsubscribing failed at JointGroupCtrlClientClient!");
    }
    
  }else{
    dPrint(3,"SpeedCtrl client already closed!");
    result = KMaCIError;
  }
  
  return result;
}
//*****************************************************************************
 
bool CJointGroupCtrlClient::SetSpeed(const float aSpeed, 
				     const int aNumberOfJoint)
{
  dPrint(10, "Sending setspeed %f %d", aSpeed, aNumberOfJoint);
  TJointSpeedArray array;
  array.push_back(TJointSpeed(aSpeed,aNumberOfJoint));
  return SetGroupSpeed(array);
}

//*****************************************************************************

bool CJointGroupCtrlClient::SetPosition(const float aPosition, 
					const int aNumberOfJoint)
{
  dPrint(10, "Sending setposition %f %d", aPosition, aNumberOfJoint);
  TJointPositionArray array;
  array.push_back(TJointPosition(aPosition,aNumberOfJoint));
  return SetGroupPosition(array);


}
//*****************************************************************************

bool CJointGroupCtrlClient::SetForce(const float aForce, 
                                     const int aNumberOfJoint)
{
  dPrint(10, "Sending setforce %f %d", aForce, aNumberOfJoint);
  TJointForceArray array;
  array.push_back(TJointForce(aForce,aNumberOfJoint));
  return SetGroupForce(array);


}
//*****************************************************************************

bool CJointGroupCtrlClient::GetSpeed(const int aJointNumber,float &aSpeed,unsigned int aTimeout_ms)
{
  bool result = false;
  if(iEventTypes.IsSet(KTypeJointSpeed)){
    Lock();
    int index = -1;
    TJointSpeedPtrArray array = iSpeedData.GetJointSpeedPtrArray();
    for(unsigned int i = 0; i< array.size();i++){
      if(array[i]->jointNumber == (unsigned int)aJointNumber){
        index =i;
        break;
      }

    }
    if(index != -1){
      aSpeed = array[index]->speed;
     
      result = true;
    }else{
      result = false;
    }
    Unlock();

  }else{
    int r;

    const unsigned int begin = ownTime_get_ms();
    int messageId = 0;
    CJointGroupCtrlData data;
    gim::binbag::CBinBag bb;
    r = data.EncodeTo(&bb);
    
    data.SetCommand(TCommand(KCommandGetSpeed));
    data.SetJointNumber(TJointNumber(aJointNumber));
    int timeLeft = 0;
    
    Lock();
    messageId = ++iCmdSequence;
    Unlock();
    
    // Send the message to default target with default major & minor numbers.
    dPrint(10,"Sending request get joint speed");
    r = iGIMIPtr->send(iDefaultTarget,
                       bb.GetBuffer(),
                       bb.GetBufferSize(),
                       iInterfaceMajor,
                       iInterfaceMinor,
                       messageId);
    dPrint(10,"Waiting messageId %d", messageId);
    Lock();
    switch(r) {
    case GIMI_OK:{
      // Message sent OK. Waiting for reply
      timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
      std::map<int,CJointGroupCtrlData *>::iterator iter = iReplyMessages.end();
      
      while (timeLeft > 0 && ( iter = iReplyMessages.find(messageId) ) == iReplyMessages.end()) {
        Wait(timeLeft,KReplySpeed);
        
        //Check for timeout
        timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
      }
      if(iter != iReplyMessages.end()){
        //get the reply data and delete it from map
        TJointSpeedPtrArray ar = iter->second->GetJointSpeedPtrArray();
        aSpeed = ar[0]->speed;     
        
        //Delete replymessage from the map
        delete iter->second;
        
        iReplyMessages.erase(iter);      
        result = true;
      }else{
        dPrint(3,"No result within timeout");
        
      }
      break;
    }
    case GIMI_NOCONNECTION:
      dPrint(3,"NO CONNECTION ");
      
      break;
      
    case GIMI_INTERRUPT:
    case GIMI_ERROR:
    default:
      // Any error, print warning and sleep for one second.
      dPrint(6,"GIMI Error (%d, '%s') - from send() received",
             r, gimi::getGimiErrorString(r).c_str());
      
      break;
    }
    Unlock();
    
    
  }
  return result;
}
//*****************************************************************************

bool CJointGroupCtrlClient::GetPosition(const int aJointNumber, 
                                        float &aPosition,
                                        unsigned int aTimeout_ms)
{

  bool result = false;
  if(iEventTypes.IsSet(KTypeJointPosition)){
    Lock();
  

    int index = -1;
    TJointPositionPtrArray array = iPositionData.GetJointPositionPtrArray();
    for(unsigned int i = 0; i< array.size();i++){
      if(array[i]->jointNumber == (unsigned int)aJointNumber){
        index =i;
        break;
      }

    }
    if(index != -1){
      aPosition = array[index]->position;
     
      result = true;
    }else{
      result = false;
    }
   
    Unlock();
    
    
  }else{
    int r;
  
    const unsigned int begin = ownTime_get_ms();
    int messageId = 0;
    CJointGroupCtrlData data;
    gim::binbag::CBinBag bb;
    r = data.EncodeTo(&bb);
    
    data.SetCommand(TCommand(KCommandGetPosition));
    data.SetJointNumber(TJointNumber(aJointNumber));
    int timeLeft = 0;
    
    Lock();
    messageId = ++iCmdSequence;
    Unlock();
    
    // Send the message to default target with default major & minor numbers.
    dPrint(10,"Sending request get joint speed");
    r = iGIMIPtr->send(iDefaultTarget,
                       bb.GetBuffer(),
                       bb.GetBufferSize(),
                       iInterfaceMajor,
                       iInterfaceMinor,
                       messageId);
    dPrint(10,"Waiting messageId %d", messageId);
    Lock();
    switch(r) {
    case GIMI_OK: {
      
      // Message sent OK. Waiting for reply
      timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
      std::map<int,CJointGroupCtrlData *>::iterator iter = iReplyMessages.end();
      
      while (timeLeft > 0 && ( iter = iReplyMessages.find(messageId) ) == iReplyMessages.end()) {
        Wait(timeLeft,KReplyPosition);
        
        //Check for timeout
        timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
      }
      
      if(iter != iReplyMessages.end()) {
        //get the reply data and delete it from map
        TJointPositionPtrArray ar = iter->second->GetJointPositionPtrArray();
        aPosition = ar[0]->position;     
        
        //Delete replymessage from the map
        delete iter->second;
        
        iReplyMessages.erase(iter);      
        result = true;
      }else{
        dPrint(3,"No result within timeout");
        
      }
      break;
    }
      
    case GIMI_NOCONNECTION:
      dPrint(3,"NO CONNECTION ");
    
      break;
      
    case GIMI_INTERRUPT:
    case GIMI_ERROR:
    default:
      // Any error, print warning and sleep for one second.
      dPrint(6,"GIMI Error (%d, '%s') - from send() received",
             r, gimi::getGimiErrorString(r).c_str());

      break;
    }
    Unlock();
  }
  


  return result;


}     
//*****************************************************************************

bool CJointGroupCtrlClient::GetForce(const int aJointNumber, 
                                     float &aForce,
                                     unsigned int aTimeout_ms)
{
  bool result = false;
  if(iEventTypes.IsSet(KTypeJointForce)){
    Lock();
  

    int index = -1;
    TJointForcePtrArray array = iForceData.GetJointForcePtrArray();
    for(unsigned int i = 0; i< array.size();i++){
      if(array[i]->jointNumber == (unsigned int)aJointNumber){
        index =i;
        break;
      }

    }
    if(index != -1){
      aForce = array[index]->force;
     
      result = true;
    }else{
      result = false;
    }
   
    Unlock();


  }else{
    int r;
 
    const unsigned int begin = ownTime_get_ms();
    int messageId = 0;
    CJointGroupCtrlData data;
    gim::binbag::CBinBag bb;
    r = data.EncodeTo(&bb);
    
    data.SetCommand(TCommand(KCommandGetForce));
    data.SetJointNumber(TJointNumber(aJointNumber));
    int timeLeft = 0;
    
    Lock();
    messageId = ++iCmdSequence;
    Unlock();
    
    // Send the message to default target with default major & minor numbers.
    dPrint(10,"Sending request get joint speed");
    r = iGIMIPtr->send(iDefaultTarget,
                       bb.GetBuffer(),
                       bb.GetBufferSize(),
                       iInterfaceMajor,
                       iInterfaceMinor,
                       messageId);
    dPrint(10,"Waiting messageId %d", messageId);
    Lock();
    switch(r) {
    case GIMI_OK: {
      
      // Message sent OK. Waiting for reply
      timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
      std::map<int,CJointGroupCtrlData *>::iterator iter = iReplyMessages.end();
      
      while (timeLeft > 0 && ( iter = iReplyMessages.find(messageId) ) == iReplyMessages.end()) {
        Wait(timeLeft,KReplyForce);
        
        //Check for timeout
        timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
      }
      
      if(iter != iReplyMessages.end()) {
        //get the reply data and delete it from map
        TJointForcePtrArray ar = iter->second->GetJointForcePtrArray();
        aForce = ar[0]->force;     
    
        
        //Delete replymessage from the map
        delete iter->second;
        
        iReplyMessages.erase(iter);      
        result = true;
      }else{
        dPrint(3,"No result within timeout");
        
      }
      break;
    }
      
    case GIMI_NOCONNECTION:
      dPrint(3,"NO CONNECTION ");
      
      break;
      
    case GIMI_INTERRUPT:
    case GIMI_ERROR:
    default:
      // Any error, print warning and sleep for one second.
      dPrint(6,"GIMI Error (%d, '%s') - from send() received",
             r, gimi::getGimiErrorString(r).c_str());
      
      break;
    }
    Unlock();
    
    
  }
  return result;
  
  
}     
//*****************************************************************************

bool CJointGroupCtrlClient::GetJointGroupInformation(TJointGroupInformation &aInfo,
						     unsigned int aTimeout_ms)
{
   

  aInfo.clear();
  int r;
  bool result = false;
  const unsigned int begin = ownTime_get_ms();
  int messageId = 0;
  CJointGroupCtrlData data;
  gim::binbag::CBinBag bb;
  r = data.EncodeTo(&bb);

  data.SetCommand(TCommand(KCommandGetJointInformation));
  int timeLeft = 0;

  Lock();
  messageId = ++iCmdSequence;
  Unlock();

  // Send the message to default target with default major & minor numbers.
  dPrint(10,"Sending request get JointGroupInformation");
  r = iGIMIPtr->send(iDefaultTarget,
                     bb.GetBuffer(),
                     bb.GetBufferSize(),
                     iInterfaceMajor,
                     iInterfaceMinor,
		     messageId);
  dPrint(10,"Waiting messageId %d", messageId);
  Lock();
  switch(r) {
  case GIMI_OK:{

    // Message sent OK. Waiting for reply
    timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
    std::map<int,CJointGroupCtrlData *>::iterator iter = iReplyMessages.end();

    while (timeLeft > 0 && ( iter = iReplyMessages.find(messageId) ) == iReplyMessages.end()) {
      Wait(timeLeft,KReplyJointInfo);
    
      //Check for timeout
      timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
    }

    if(iter != iReplyMessages.end()){
      //Transforma the TJointInformation_Payload to TJointInfo and delete replymsg from map
     	     
      unsigned int jointCount = iter->second->GetJointInformationHeader()->numberOfJoints;
      ///Transform the TJointInformation_Payload to TJointInfo
      for(unsigned int i = 0; i< jointCount; i++){
	std::string name = std::string((iter->second->GetJointInformation(i)->name));
	int jointNumber = (iter->second->GetJointInformation(i)->jointNumber);
	EJointControlType jointControlType = (EJointControlType)(iter->second->GetJointInformation(i)->jointControlType);
	bool canControlSpeed = true;
	if(iter->second->GetJointInformation(i)->canControlSpeed == 0){
	  canControlSpeed = false;
	}
	float maxSpeed = iter->second->GetJointInformation(i)->maxSpeed;
	bool canControlPosition = true;
	if(iter->second->GetJointInformation(i)->canControlPosition == 0){
	  canControlPosition = false;
	}
	float minPosition = iter->second->GetJointInformation(i)->minPosition;
	float maxPosition = iter->second->GetJointInformation(i)->maxPosition;

        bool canControlForce = true;
        if(iter->second->GetJointInformation(i)->canControlForce == 0){
	  canControlForce = false;
	}
	float minForce = iter->second->GetJointInformation(i)->minForce;
	float maxForce = iter->second->GetJointInformation(i)->maxForce;
	TJointInfo info(name, 
			jointNumber,
			jointControlType,
			canControlSpeed,
			maxSpeed,
			canControlPosition,
			minPosition,
			maxPosition,
                        canControlForce,
                        minForce,
                        maxForce);
	aInfo.push_back(info);
      }
      //Delete replymessage from the map
      delete iter->second;

      iReplyMessages.erase(iter);      
      result = true;
    }else{
      dPrint(3,"No result within timeout");

    }
    break;
  }
  case GIMI_NOCONNECTION:
    dPrint(3,"NO CONNECTION ");
    
    break;
    
  case GIMI_INTERRUPT:
  case GIMI_ERROR:
  default:
    // Any error, print warning and sleep for one second.
    dPrint(6,"GIMI Error (%d, '%s') - from send() received",
           r, gimi::getGimiErrorString(r).c_str());

    break;
  }
  Unlock();
  


  return result;
}
//*****************************************************************************

int CJointGroupCtrlClient::GetJointNumber(void)
{
	int number = 0;
	TJointGroupInformation data;
	if (GetJointGroupInformation(data, 1000)){
		number = data.size();
	}
	return number;
}
//*****************************************************************************

bool CJointGroupCtrlClient::GetJointInformation(TJointInfo &aInfo, int aNumber,
				    unsigned int aTimeout_ms)
{
	bool ret;
//	int number = 0;
//	struct TJointInfo theJoint;
	TJointGroupInformation data;
	ret = GetJointGroupInformation(data, aTimeout_ms);
	if (ret){
		aInfo = data.at(aNumber);
	}
	return ret;	
}
//*****************************************************************************

bool CJointGroupCtrlClient::GetJointGroupStatus(EJointGroupStatus &aStatus,
                                                unsigned int aTimeout_ms)
{

  int r;
  bool result = false;
  const unsigned int begin = ownTime_get_ms();
  int messageId = 0;
  CJointGroupCtrlData data;
  gim::binbag::CBinBag bb;
  r = data.EncodeTo(&bb);
  
  data.SetCommand(TCommand(KCommandGetJointGroupStatus));
  int timeLeft = 0;

  Lock();
  messageId = ++iCmdSequence;
  Unlock();

  // Send the message to default target with default major & minor numbers.
  dPrint(10,"Sending request get JointGroupStatus");
  r = iGIMIPtr->send(iDefaultTarget,
                     bb.GetBuffer(),
                     bb.GetBufferSize(),
                     iInterfaceMajor,
                     iInterfaceMinor,
		     messageId);
  dPrint(10,"Waiting messageId %d", messageId);
  Lock();
  switch(r) {
  case GIMI_OK:{

    // Message sent OK. Waiting for reply
    timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
    std::map<int,CJointGroupCtrlData *>::iterator iter = iReplyMessages.end();
    while (timeLeft > 0 && ( iter = iReplyMessages.find(messageId) ) == iReplyMessages.end()) {
      Wait(timeLeft,KReplyJointGroupStatus);

      //Check for timeout
      timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
    }
    if(iter != iReplyMessages.end()){
      //get the reply data and delete it from map
    
      aStatus = (EJointGroupStatus)iter->second->GetJointGroupStatus()->status;

      //Delete replymessage from the map
      delete iter->second;
      
      iReplyMessages.erase(iter);      
      result = true;
    }else{
      dPrint(3,"No result within timeout");

    }
    break;
  }
  case GIMI_NOCONNECTION:
    dPrint(3,"NO CONNECTION ");
    
    break;
    
  case GIMI_INTERRUPT:
  case GIMI_ERROR:
  default:
    // Any error, print warning and sleep for one second.
    dPrint(6,"GIMI Error (%d, '%s') - from send() received",
           r, gimi::getGimiErrorString(r).c_str());

    break;
  }
  Unlock();
  


  return result;  

}
//*****************************************************************************

bool CJointGroupCtrlClient::SetGroupSpeed(TJointSpeedArray &aJointSpeedArray)
{

  // Construct command to set speed
  gim::binbag::CBinBag bb;
  CJointGroupCtrlData ctrl;
  ctrl.EncodeTo(&bb);
  ctrl.SetCommand(TCommand(KCommandSetGroupSpeeds));
  for(EACH_IN_i(aJointSpeedArray)){
    ctrl.SetSpeed(*i);
  }
  
  // Send the command
  const EMaCIError e = SendInterfaceMessage(bb.GetBuffer(),
                                            bb.GetBufferSize());

  return (e == KMaCIOK);

}
//*****************************************************************************

bool CJointGroupCtrlClient::SetGroupPosition(TJointPositionArray &aJointPositionArray)
{

  // Construct command to set speed
  gim::binbag::CBinBag bb;
  CJointGroupCtrlData ctrl;
  ctrl.EncodeTo(&bb);
  ctrl.SetCommand(TCommand(KCommandSetGroupPositions));
  for(EACH_IN_i(aJointPositionArray)){
    ctrl.SetPosition(*i);
  }
  
  // Send the command
  const EMaCIError e = SendInterfaceMessage(bb.GetBuffer(),
                                            bb.GetBufferSize());

  return (e == KMaCIOK);

}
//*****************************************************************************

bool CJointGroupCtrlClient::SetGroupForce(TJointForceArray &aJointForceArray)
{
 // Construct command to set speed
  gim::binbag::CBinBag bb;
  CJointGroupCtrlData ctrl;
  ctrl.EncodeTo(&bb);
  ctrl.SetCommand(TCommand(KCommandSetGroupForces));
  for(EACH_IN_i(aJointForceArray)){
    ctrl.SetForce(*i);
  }
  
  // Send the command
  const EMaCIError e = SendInterfaceMessage(bb.GetBuffer(),
                                            bb.GetBufferSize());

  return (e == KMaCIOK);

}
//*****************************************************************************

bool CJointGroupCtrlClient::GetGroupSpeeds(TJointSpeedArray &aJointSpeedArray,
                                          unsigned int *aSeq,
                                          unsigned int aTimeout_ms)
{
  dPrint(1,"Get group speeds called eventTypeSet =  %d",iEventTypes.IsSet(KTypeJointSpeed));
  bool result = false;
  //This is not sent as an event, so poll the answer...
  if(!iEventTypes.IsSet(KTypeJointSpeed)){
    int r;
    
    const unsigned int begin = ownTime_get_ms();
    int messageId = 0;
    CJointGroupCtrlData data;
    gim::binbag::CBinBag bb;
    r = data.EncodeTo(&bb);
    
    data.SetCommand(TCommand(KCommandGetGroupSpeeds));
   
    int timeLeft = 0;
    
    Lock();
    messageId = ++iCmdSequence;
    Unlock();
    
    // Send the message to default target with default major & minor numbers.
    dPrint(10,"Sending request get group speeds");
    r = iGIMIPtr->send(iDefaultTarget,
                       bb.GetBuffer(),
                       bb.GetBufferSize(),
                       iInterfaceMajor,
                       iInterfaceMinor,
                       messageId);
    dPrint(10,"Waiting messageId %d", messageId);

    switch(r) {
    case GIMI_OK:{

      // Message sent OK. Waiting for reply
    timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
    std::map<int,CJointGroupCtrlData *>::iterator iter = iReplyMessages.end();
    while (timeLeft > 0 && ( iter = iReplyMessages.find(messageId) ) == iReplyMessages.end()) {
      Wait(timeLeft,KGroupSpeeds);
      
      //Check for timeout
      timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
    }
    
    if(iter != iReplyMessages.end()){
      //get the reply data and delete it from map
      TJointSpeedPtrArray array = iter->second->GetJointSpeedPtrArray();
      
      for(EACH_IN_i(array)){
        aJointSpeedArray.push_back(TJointSpeed((*i)->speed,(*i)->jointNumber));
        
      }
      //Delete replymessage from the map
      delete iter->second;
      
      iReplyMessages.erase(iter);      
      result = true;
    }else{
      dPrint(3,"No result within timeout");
      
    }
    break;
    }
    case GIMI_NOCONNECTION:
      dPrint(3,"NO CONNECTION ");
      return false;
      break;
      
    case GIMI_INTERRUPT:
    case GIMI_ERROR:
    default:
      // Any error, print warning and sleep for one second.
      dPrint(6,"GIMI Error (%d, '%s') - from send() received",
             r, gimi::getGimiErrorString(r).c_str());
      return false;
      break;
    }

  }else{
    dPrint(1,"aSeqPtr value %d, aSeq %d, iSpeedDataSequence", aSeq,*aSeq,iSpeedDataSequence)
    ///////////////////////////////////////////////////////////
    ////////////////////EVENT MECHANISM////////////////////////
    ///////////////////////////////////////////////////////////
    Lock();
    //No waiting...
    if(aSeq == NULL){

      TJointSpeedPtrArray array = iSpeedData.GetJointSpeedPtrArray();
      
      for(EACH_IN_i(array)){
        aJointSpeedArray.push_back(TJointSpeed((*i)->speed,(*i)->jointNumber));
        
      }

      result = true;
    }else{
      if(iSpeedDataSequence <= *aSeq){
        int time_left = aTimeout_ms;
        unsigned int time_begin = ownTime_get_ms();
        // The available scan is older than requested - must wait for time_left
      do {
	Wait(time_left,  KGroupSpeeds);
	time_left = aTimeout_ms - ownTime_get_ms_since(time_begin);
	
      } while(IsActive() && iSpeedDataSequence < *aSeq && time_left > 0);

      // Now, check whether the waiting resulted in the correct sequence
      if (iSpeedDataSequence > *aSeq) {
        TJointSpeedPtrArray array = iSpeedData.GetJointSpeedPtrArray();
      
        for(EACH_IN_i(array)){
          aJointSpeedArray.push_back(TJointSpeed((*i)->speed,(*i)->jointNumber));
          
        }
        *aSeq = iSpeedDataSequence;
        result = true;
        
      } else {
	// No :(
	dPrint(6, "Failed to get JointGroupSpeeds! (Timeout %dms)",
	       aTimeout_ms);
      }

      }else{
        TJointSpeedPtrArray array = iSpeedData.GetJointSpeedPtrArray();
      
        for(EACH_IN_i(array)){
          aJointSpeedArray.push_back(TJointSpeed((*i)->speed,(*i)->jointNumber));
          
        }
        *aSeq = iSpeedDataSequence;
        result = true;
        
        
      }

    }

    Unlock();
  }
  return result;  




}
//*****************************************************************************

bool CJointGroupCtrlClient::GetGroupPositions(TJointPositionArray &aJointPositionArray,
                                             unsigned int *aSeq,
                                             unsigned int aTimeout_ms)

{
  dPrint(1,"Get group position called eventTypeSet =  %d",iEventTypes.IsSet(KTypeJointPosition));
  bool result = false;
  if(!iEventTypes.IsSet(KTypeJointPosition)){
    int r;
    
    const unsigned int begin = ownTime_get_ms();
    int messageId = 0;
    CJointGroupCtrlData data;
    gim::binbag::CBinBag bb;
    r = data.EncodeTo(&bb);
    
    data.SetCommand(TCommand(KCommandGetGroupPositions));
    
    int timeLeft = 0;
    
    Lock();
    messageId = ++iCmdSequence;
    Unlock();
    
    // Send the message to default target with default major & minor numbers.
    dPrint(10,"Sending request get group positions");
    r = iGIMIPtr->send(iDefaultTarget,
                       bb.GetBuffer(),
                       bb.GetBufferSize(),
                       iInterfaceMajor,
                       iInterfaceMinor,
                       messageId);
    dPrint(10,"Waiting messageId %d", messageId);
    Lock();
    switch(r) {
    case GIMI_OK:{
      
      // Message sent OK. Waiting for reply
      timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
      std::map<int,CJointGroupCtrlData *>::iterator iter = iReplyMessages.end();
      while (timeLeft > 0 && ( iter = iReplyMessages.find(messageId) ) == iReplyMessages.end()) {
        Wait(timeLeft,KGroupPositions);
        
        //Check for timeout
        timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
      }
      
      if(iter != iReplyMessages.end()){
        //get the reply data and delete it from map
        TJointPositionPtrArray array = iter->second->GetJointPositionPtrArray();
        
        for(EACH_IN_i(array)){
          aJointPositionArray.push_back(TJointPosition((*i)->position,(*i)->jointNumber));
          
        }
        //Delete replymessage from the map
        delete iter->second;
        
        iReplyMessages.erase(iter);      
        result = true;
      }else{
        dPrint(3,"No result within timeout");
        
      }
      break;
    }
    case GIMI_NOCONNECTION:
      dPrint(3,"NO CONNECTION ");
      
      break;
      
    case GIMI_INTERRUPT:
    case GIMI_ERROR:
    default:
      // Any error, print warning and sleep for one second.
    dPrint(6,"GIMI Error (%d, '%s') - from send() received",
           r, gimi::getGimiErrorString(r).c_str());
    
    break;
    }
    Unlock();
  }else{
    dPrint(1,"aSeqPtr value %d, aSeq %d, iPositionDataSequence", aSeq,*aSeq,iPositionDataSequence)
    ///////////////////////////////////////////////////////////
    ////////////////////EVENT MECHANISM////////////////////////
    ///////////////////////////////////////////////////////////
    Lock();
    //No waiting...
    if(aSeq == NULL){

      TJointPositionPtrArray array = iPositionData.GetJointPositionPtrArray();
      
      for(EACH_IN_i(array)){
        aJointPositionArray.push_back(TJointPosition((*i)->position,(*i)->jointNumber));
        
      }

      result = true;
    }else{
      if(iPositionDataSequence <= *aSeq){
        int time_left = aTimeout_ms;
        unsigned int time_begin = ownTime_get_ms();
        // The available scan is older than requested - must wait for time_left
      do {
	Wait(time_left,  KGroupPositions);
	time_left = aTimeout_ms - ownTime_get_ms_since(time_begin);
	
      } while(IsActive() && iPositionDataSequence < *aSeq && time_left > 0);

      // Now, check whether the waiting resulted in the correct sequence
      if (iPositionDataSequence > *aSeq) {
        TJointPositionPtrArray array = iPositionData.GetJointPositionPtrArray();
      
        for(EACH_IN_i(array)){
          aJointPositionArray.push_back(TJointPosition((*i)->position,(*i)->jointNumber));
          
        }
        *aSeq = iPositionDataSequence;
        result = true;
        
      } else {
	// No :(
	dPrint(6, "Failed to get JointGroupSpeeds! (Timeout %dms)",
	       aTimeout_ms);
      }

      }else{
        TJointPositionPtrArray array = iPositionData.GetJointPositionPtrArray();
      
        for(EACH_IN_i(array)){
          aJointPositionArray.push_back(TJointPosition((*i)->position,(*i)->jointNumber));
          
        }
        *aSeq = iPositionDataSequence;
        result = true;
        
        
      }

    }

    Unlock();
  }


  return result;  



}
//*****************************************************************************

bool CJointGroupCtrlClient::GetGroupForces(TJointForceArray &aJointForceArray,
                                          unsigned int *aSeq,
                                          unsigned int aTimeout_ms)

{
  dPrint(1,"Get group forces called eventTypeSet =  %d",iEventTypes.IsSet(KTypeJointForce));
  bool result = false;
  if(!iEventTypes.IsSet(KTypeJointForce)){

    int r;
    
    const unsigned int begin = ownTime_get_ms();
    int messageId = 0;
    CJointGroupCtrlData data;
    gim::binbag::CBinBag bb;
    r = data.EncodeTo(&bb);
    
    data.SetCommand(TCommand(KCommandGetGroupForces));
    
    int timeLeft = 0;
    
    Lock();
    messageId = ++iCmdSequence;
    Unlock();
    
    // Send the message to default target with default major & minor numbers.
    dPrint(1,"Sending request get group forces");
    r = iGIMIPtr->send(iDefaultTarget,
                       bb.GetBuffer(),
                       bb.GetBufferSize(),
                       iInterfaceMajor,
                       iInterfaceMinor,
                       messageId);
    dPrint(1,"Waiting messageId %d", messageId);
    Lock();
    switch(r) {
    case GIMI_OK:{
    
    // Message sent OK. Waiting for reply
    timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
    std::map<int,CJointGroupCtrlData *>::iterator iter = iReplyMessages.end();
    while (timeLeft > 0 && ( iter = iReplyMessages.find(messageId) ) == iReplyMessages.end()) {
      Wait(timeLeft,KGroupForces);

      //Check for timeout
      timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
    }

    if(iter != iReplyMessages.end()){
      dPrint(1,"Found message...");
      //get the reply data and delete it from map
      TJointForcePtrArray array = iter->second->GetJointForcePtrArray();

      for(EACH_IN_i(array)){
        aJointForceArray.push_back(TJointForce((*i)->force,(*i)->jointNumber));

      }
      //Delete replymessage from the map
      delete iter->second;
      
      iReplyMessages.erase(iter);      
      result = true;
    }else{
      dPrint(3,"No result within timeout");

    }
    break;
    }
    case GIMI_NOCONNECTION:
      dPrint(3,"NO CONNECTION ");
      
      break;
      
    case GIMI_INTERRUPT:
    case GIMI_ERROR:
    default:
      // Any error, print warning and sleep for one second.
      dPrint(6,"GIMI Error (%d, '%s') - from send() received",
             r, gimi::getGimiErrorString(r).c_str());
      
      break;
    }
  Unlock();
  

  }else{
    ///////////////////////////////////////////////////////////
    ////////////////////EVENT MECHANISM////////////////////////
    ///////////////////////////////////////////////////////////
    dPrint(1,"aSeqPtr value %d, aSeq %d, iForceDataSequence", aSeq,*aSeq,iForceDataSequence)
    Lock();
    //No waiting...
    if(aSeq == NULL){

      TJointForcePtrArray array = iForceData.GetJointForcePtrArray();
      
      for(EACH_IN_i(array)){
        aJointForceArray.push_back(TJointForce((*i)->force,(*i)->jointNumber));
        
      }

      result = true;
    }else{
      if(iForceDataSequence <= *aSeq){
        int time_left = aTimeout_ms;
        unsigned int time_begin = ownTime_get_ms();
        // The available scan is older than requested - must wait for time_left
      do {
	Wait(time_left,  KGroupForces);
	time_left = aTimeout_ms - ownTime_get_ms_since(time_begin);
	
      } while(IsActive() && iForceDataSequence < *aSeq && time_left > 0);

      // Now, check whether the waiting resulted in the correct sequence
      if (iForceDataSequence > *aSeq) {
        TJointForcePtrArray array = iForceData.GetJointForcePtrArray();
      
        for(EACH_IN_i(array)){
          aJointForceArray.push_back(TJointForce((*i)->force,(*i)->jointNumber));
          
        }
        *aSeq = iForceDataSequence;
        result = true;
        
      } else {
	// No :(
	dPrint(6, "Failed to get JointGroupSpeeds! (Timeout %dms)",
	       aTimeout_ms);
      }

      }else{
        TJointForcePtrArray array = iForceData.GetJointForcePtrArray();
      
        for(EACH_IN_i(array)){
          aJointForceArray.push_back(TJointForce((*i)->force,(*i)->jointNumber));
          
        }
        *aSeq = iForceDataSequence;
        result = true;
        
        
      }

    }

    Unlock();
  }
  return result;  

 

}
//*****************************************************************************

int CJointGroupCtrlClient::ThreadFunction(const int aThreadNumber)
{
  int result = 0;
  EMaCIError e;
  gimi::GIMIMessage gmsg;
  
  assert(aThreadNumber == 0);
  assert(iGIMIPtr != NULL);
  
  dPrint(4, "GIMIMessage handler started");
   // Run as long as the thread should be active
  while(IsActive()) {
    // Call the ReceiveMessage wrapper with 1s timeout
    e = ReceiveInterfaceMessage(gmsg, 1000*60*60*24);
    if (e == KMaCIOK) {
     
      ProcessMessage(gmsg);


    } else if (e != KMaCITimeout) {
      if(IsActive()){
        // Other error, try to recover by sleeping one second.
        dPrint(3,"ProcessMessage returned error %u and client still active. Trying to recover by sleeping 1000ms ",
               e);
        ownSleep_ms(1000);
      }else{
        //
        dPrint(3,"ProcessMessage returned error %u. Normal situtation because client is already closed",
               e);
      }
      
    }
  }
  
  dPrint(4, "GIMIMessage handler stopped");

  return result;
}
//*****************************************************************************
int CJointGroupCtrlClient::ProcessMessage(const gimi::GIMIMessage &msg)
{
int result = 0;
  gim::binbag::CBinBag bb;
  CJointGroupCtrlData c;
  bool r;


  r = bb.Decode(msg.getData(), msg.getDatalength());

  if (r) {
    // Succesfully decoded as BinBag.
    r = c.DecodeFrom(&bb);
    if (r) {
      // Succesfully decoded as SpeedCtrl
      // Now, Check whether it was Command.
      const TCommand *cmd = c.GetCommand();
      if (cmd) {
        switch(cmd->cmd) {
	
	  //Getting a point reached event
	case KReplyGetJointInformation:{
	  dPrint(8,"Received reply for joint information query");
	  CJointGroupCtrlData * new_entry = new CJointGroupCtrlData(c);
	  new_entry->Print(10);
	  Lock();
	  iReplyMessages.insert(std::make_pair(msg.messageId,new_entry));
	  Notify(KReplyJointInfo);
	  Unlock();
	  break;
	}
	case KReplyGetPosition:{
	  dPrint(8,"Received GetPosition reply");
	  CJointGroupCtrlData * new_entry = new CJointGroupCtrlData(c);
	  new_entry->Print(10);
	  Lock();
          dPrint(1,"Copying!?");
	  iReplyMessages.insert(std::make_pair(msg.messageId,new_entry));
          dPrint(1,"endcopying!?");
	  Notify(KReplyPosition);
	  Unlock();
	  break;
	}
	case KReplyGetSpeed:{
	  dPrint(8,"Received GetSpeed reply");
	  CJointGroupCtrlData * new_entry = new CJointGroupCtrlData(c);
	  new_entry->Print(10);
	  Lock();
	  iReplyMessages.insert(std::make_pair(msg.messageId,new_entry));
	  Notify(KReplySpeed);
	  Unlock();
	  break;
	}

	case KReplyGetForce:{
	  dPrint(8,"Received GetForce reply");
	  CJointGroupCtrlData * new_entry = new CJointGroupCtrlData(c);
	  new_entry->Print(10);
	  Lock();
	  iReplyMessages.insert(std::make_pair(msg.messageId,new_entry));
	  Notify(KReplyForce);
	  Unlock();
	  break;
	}

        case  KReplyGetJointGroupStatus:{
          dPrint(8,"Received Get Joint group status reply");
	  CJointGroupCtrlData * new_entry = new CJointGroupCtrlData(c);
	  new_entry->Print(10);
	  Lock();
	  iReplyMessages.insert(std::make_pair(msg.messageId,new_entry));
	  Notify(KReplyJointGroupStatus);
	  Unlock();
	  break;
        }
        case  KReplyGetGroupPositions:{
          dPrint(8,"Received Get Joint group positions reply");
	  CJointGroupCtrlData * new_entry = new CJointGroupCtrlData(c);
	  new_entry->Print(10);
	  Lock();
	  iReplyMessages.insert(std::make_pair(msg.messageId,new_entry));
	  Notify(KGroupPositions);
	  Unlock();
	  break;
        }
        case  KReplyGetGroupSpeeds:{
          dPrint(8,"Received Get Joint group speeds reply");
	  CJointGroupCtrlData * new_entry = new CJointGroupCtrlData(c);
	  new_entry->Print(10);
	  Lock();
	  iReplyMessages.insert(std::make_pair(msg.messageId,new_entry));
	  Notify(KGroupSpeeds);
	  Unlock();
	  break;
        }
        case  KReplyGetGroupForces:{
          dPrint(8,"Received Get Joint group positions reply");
	  CJointGroupCtrlData * new_entry = new CJointGroupCtrlData(c);
	  new_entry->Print(10);
	  Lock();
	  iReplyMessages.insert(std::make_pair(msg.messageId,new_entry));
	  Notify(KGroupForces);
	  Unlock();
	  break;
        }
        case KCommandEvent:{
          dPrint(8,"Received an event");
	  
	  Lock();
          if(c.GetJointPositionPtrArray().size() >0){
            iPositionData.Reset();
            iPositionData = c;
            iPositionDataSequence++;
            Notify(KGroupPositions);
          }

          if(c.GetJointSpeedPtrArray().size() >0){
            iSpeedData.Reset();
            iSpeedData = c;
            iSpeedDataSequence++;
            Notify(KGroupSpeeds);
          }

          if(c.GetJointForcePtrArray().size() >0){
            iForceData.Reset();
            iForceData = c;
            iForceDataSequence++;
            Notify(KGroupForces);
          }


          Unlock();
          break;
        }
	default:
          dPrint(4,"Unidentified command! %d (%x)! Ignore.",
                 cmd->cmd, cmd->cmd);
          break;
        }
      }
    }else{
      dPrint(5,"DecodeFrom failed");
    }
  }else{
    dPrint(5,"Decode failed");
  }
  
  
  return result;

}
//*****************************************************************************

bool CJointGroupCtrlClient::GetEventTypes()
{
  bool result;
  int r;
  int messageId = 0;
  CJointGroupCtrlData jgcData;
  gim::binbag::CBinBag bb;
  r = jgcData.EncodeTo(&bb);
  gimi::GIMIMessage gmsg;
  jgcData.SetCommand(TCommand(KCommandGetEventTypes));
  Lock();
  messageId = ++iCmdSequence;
  Unlock();
    // Send the message to default target with default major & minor numbers.
  dPrint(10,"Sending request get event types");
  r = iGIMIPtr->send(iDefaultTarget,
                     bb.GetBuffer(),
                     bb.GetBufferSize(),
                     iInterfaceMajor,
                     iInterfaceMinor,
                     messageId);

  switch(r) {
  case GIMI_OK:{

    r = iGIMIPtr->receive(gmsg,
			  1000,
			  iInterfaceMajor,
			  iInterfaceMinor,
			  iRedirectionBufferId);
    switch(r) {
    case GIMI_OK:{
      // Got GIMI OK, so save the 
      gim::binbag::CBinBag bbReply;
      CJointGroupCtrlData jgcDataReply;
    
      result = bbReply.Decode(gmsg.getData(), gmsg.getDatalength());
      if (result) {
        // Succesfully decoded as BinBag.
        jgcDataReply.Reset();
        result =  jgcDataReply.DecodeFrom(&bbReply);
        if (result) {

          // Get timestamp element
          const TEventTypes *et = jgcDataReply.GetEventTypes();
          if (et != NULL) {
            // If the element is found, convert it to gim::time
            iEventTypes= et->continuousTypes;
          }else{
            dPrint(3,"Failed to get eventtypes from reply");
            return false;
          }
        }else{
          dPrint(3,"Failed to decode from binbag. Reply reading failed");
          return false;
        }
      }else{
        dPrint(3,"Failed to decode reply message to binbag");
        return false;
      }
      break;
    }
    case GIMI_TIMEOUT:
      // GIMI Timeout - Do nothing.
      break;
      
    case GIMI_INTERRUPT:
    case GIMI_ERROR:
    default:
      // Any error, print warning and sleep for one second.
      dPrint(2,"GIMI Error (%d, '%s') received. Sleeping one second.",
	     r, gimi::getGimiErrorString(r).c_str());
      ownSleep_ms(1000);
      break;
    }
    break;
  }
  case GIMI_NOCONNECTION:{
    dPrint(3,"NO CONNECTION ");
   ;
    return false;
      break;
  }
  case GIMI_INTERRUPT:
  case GIMI_ERROR:
  default:
    // Any error, print warning and sleep for one second.
    dPrint(6,"GIMI Error (%d, '%s') - from send() received",
             r, gimi::getGimiErrorString(r).c_str());
   
    return false;
    break;
  }
 
  
  
  return true;
}
//*****************************************************************************

bool CJointGroupCtrlClient::RequestStateOfServer(){
  int r;
  bool result;
  CJointGroupCtrlData jgcData;
  gim::binbag::CBinBag bb;
  r = jgcData.EncodeTo(&bb);
  gimi::GIMIMessage gmsg;
  jgcData.SetCommand(TCommand(KCommandUpdateState));
  Lock();
  
  Unlock();
    // Send the message to default target with default major & minor numbers.
  dPrint(10,"Sending request get event types");
  r = iGIMIPtr->send(iDefaultTarget,
                     bb.GetBuffer(),
                     bb.GetBufferSize(),
                     iInterfaceMajor,
                     iInterfaceMinor,
                     0);

  switch(r) {
  case GIMI_OK:{
    result = true;
    break;
  }
  default:
    dPrint(1,"Failed to send request for updating the state");
    result = false;
    break;
  } 
  
  
  return result;

}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
